21. EKF Algorithm Generalization

EKF Algortihm Generalization

Extended Kalman Filter Equations

Although the mathematical proof is somewhat complex, it turns out that the Kalman filter equations and extended Kalman filter equations are very similar. The main differences are:

  • the F matrix will be replaced by F_j when calculating P' .
  • the H matrix in the Kalman filter will be replaced by the Jacobian matrix H_j when calculating S , K , and P .
  • to calculate x' , the prediction update function, f , is used instead of the F matrix.
  • to calculate y , the h function is used instead of the H matrix.

For this project, however, we do not need to use the f function or F_j . If we had been using a non-linear model in the prediction step, we would need to replace the F matrix with its Jacobian, F_j . However, we are using a linear model for the prediction step. So, for the prediction step, we can still use the regular Kalman filter equations and the F matrix rather than the extended Kalman filter equations.

The measurement update for lidar will also use the regular Kalman filter equations, since lidar uses linear equations. Only the measurement update for the radar sensor will use the extended Kalman filter equations.

One important point to reiterate is that the equation y = z - Hx' for the Kalman filter does not become y = z - H_jx for the extended Kalman filter. Instead, for extended Kalman filters, we'll use the h function directly to map predicted locations x' from Cartesian to polar coordinates.

The comparison for reference.

The comparison for reference.

Clarification of u = 0

In the above image, the prediction equation is written as x' = Fx + u and x' = f(x,u) . Previously the equation was written x' = Fx + \nu .

It is just a question of notation where \nu is the greek letter "nu" and "u" is used in the code examples. Remember that \nu is represented by a gaussian distribution with mean zero. The equation x' = Fx + u or the equivalent equation x' = Fx + \nu calculates the mean value of the state variable x ; hence we set u = 0. The uncertainty in the gaussian distribution shows up in the Q matrix.

More Details About Calculations with Radar Versus Lidar

In the radar update step, the Jacobian matrix H_j is used to calculate S , K and P . To calculate y , we use the equations that map the predicted location x' from Cartesian coordinates to polar coordinates:

h(x')= \begin{pmatrix} \sqrt{ p{'}_{x}^2 + p{'}_{y}^2 }\\ \arctan(p_y' / p_x')\\ \frac{p_x' v_x' + p_y' v_y'}{\sqrt{p{'}_{x}^2 + p{'}_{y}^2}} \end{pmatrix}

The predicted measurement vector x' is a vector containing values in the form \begin{bmatrix} p_x, p_y, v_x, v_y \end{bmatrix} . The radar sensor will output values in polar coordinates:

\begin{pmatrix} \rho\\ \phi\\ \dot{\rho} \end{pmatrix}

In order to calculate y for the radar sensor, we need to convert x' to polar coordinates. In other words, the function h(x) maps values from Cartesian coordinates to polar coordinates. So the equation for radar becomes y = z_{radar} - h(x') .

One other important point when calculating y with radar sensor data: the second value in the polar coordinate vector is the angle \phi . You'll need to make sure to normalize \phi in the y vector so that its angle is between -\pi and \pi ; in other words, add or subtract 2\pi from \phi until it is between -\pi and \pi .

To summarize:

  • for measurement updates with lidar, we can use the H matrix for calculating y , S , K and P .
  • for radar, H_j is used to calculate S , K and P .

Compared to Kalman Filters, how would the Extended Kalman Filter result differ when the prediction function and measurement function are both linear?

SOLUTION: The Extended Kalman Filter's result would be the same as the standard Kalman Filter's result.

Next Concept